#include "mapping/2d/map_builder.h"

#include "third_party/include/math.h"
#include "third_party/include/safe_queue.h"
#include "mapping/coordinate_transform.h"

namespace cartographer {
namespace mapping {
// MapBuilder类的构造函数
// 参数initial_map_size：初始化地图的尺寸
// 参数resolution：地图的分辨率，即每个像素代表的实际距离
MapBuilder::MapBuilder(int initial_map_size, float resolution) : rclcpp::Node("map_builder") {
    p_occupancy_grid_pub_ = create_publisher<nav_msgs::msg::OccupancyGrid>("lio_sam/mapping/occupancy_grid", 1);

    p_sub_laser_cloud_ = create_subscription<sensor_msgs::msg::PointCloud2>(
        "lio_sam/deskew/cloud_deskewed", 10, std::bind(&MapBuilder::LaserCloudHandler, this, std::placeholders::_1));

    // 初始化occupancy_grid_的消息头
    occupancy_grid_.header.stamp    = rclcpp::Clock().now();
    occupancy_grid_.header.frame_id = "map";
    occupancy_grid_.info.resolution = resolution;

    // 设置地图的分辨率、宽度、高度和原点位置
    occupancy_grid_.info.width             = initial_map_size;
    occupancy_grid_.info.height            = initial_map_size;
    occupancy_grid_.info.origin.position.x = -0.5 * initial_map_size * resolution;
    occupancy_grid_.info.origin.position.y = -0.5 * initial_map_size * resolution;
    occupancy_grid_.info.origin.position.z = 0.0;
    // 初始化地图数据，使用-1表示未知空间
    std::vector<int8_t> data(occupancy_grid_.info.width * occupancy_grid_.info.height, -1);
    occupancy_grid_.data = std::move(data);

    // 配置ProbabilityGridRangeDataInserter2D的选项
    ProbabilityGridRangeDataInserterOptions2D options_2d;
    options_2d.hit_probability   = 0.55;
    options_2d.miss_probability  = 0.46;
    options_2d.insert_free_space = true;
    // 创建并初始化ProbabilityGridRangeDataInserter2D对象
    p_probability_grid_range_data_inserter_ = std::make_unique<ProbabilityGridRangeDataInserter2D>(options_2d);
    // 创建并初始化ProbabilityGrid对象，用于处理和存储概率地图数据
    p_probability_grid_ = std::make_unique<ProbabilityGrid>(
        MapLimits(resolution, 0.5 * initial_map_size * resolution * Eigen::Vector2d::Ones(),
                  CellLimits(initial_map_size, initial_map_size)));
}

void MapBuilder::LaserCloudHandler(const sensor_msgs::msg::PointCloud2::SharedPtr msg) {}

/**
 * @brief 更新地图
 *
 * 本函数通过将传感器的范围数据插入到概率网格中，并根据概率值更新occupancy grid（占用网格），
 * 以构建和维护一个概率性的地图表示。这包括将概率网格中的数据转换为occupancy grid格式，
 * 以及发布更新后的occupancy grid。
 *
 * @param range_data 传感器范围数据，包含一系列的测量点和对应的测量范围
 */
void MapBuilder::UpdateMap(const sensor::RangeData& range_data) {

    // 将传感器范围数据插入到概率网格中
    p_probability_grid_range_data_inserter_->Insert(range_data, p_probability_grid_.get());
    // 获取occupancy grid的原点坐标
    Eigen::Vector2f origin{occupancy_grid_.info.origin.position.x, occupancy_grid_.info.origin.position.y};
    // 获取待更新的网格单元数据队列
    auto& data_queue = p_probability_grid_->GetUpdateGridCellsQueue();

    // 遍历数据队列，更新occupancy grid中的对应单元
    while (!data_queue.empty()) {
        Eigen::Array2i cell_index;
        data_queue.try_pop(cell_index);
        // 获取当前网格单元的概率值
        float probability = p_probability_grid_->GetProbability(cell_index);
        // 将概率值映射到0-100的范围内
        int value = ClampedMap<float, int>(probability, 0.1, 0.9, 0, 100);
        // 将概率网格中的单元转换为世界坐标
        Eigen::Vector2f world_cell = p_probability_grid_->limits().GetWorld(cell_index);
        // 将世界坐标转换为occupancy grid中的单元索引
        Eigen::Array2i occupancy_grid_cell = World2OccpancyCell(world_cell, origin, occupancy_grid_.info.resolution);
        // 计算occupancy grid中的一维索引
        int flat_index = occupancy_grid_cell.y() * occupancy_grid_.info.width + occupancy_grid_cell.x();
        // 更新occupancy grid中的值
        occupancy_grid_.data[flat_index] = value;
    }

    occupancy_grid_.header.stamp = rclcpp::Time(range_data.time * 1e9);
    p_occupancy_grid_pub_->publish(occupancy_grid_);
}
}  // namespace mapping
}  // namespace cartographer

int main(int argc, char** argv) {
    rclcpp::init(argc, argv);

    rclcpp::NodeOptions options;
    options.use_intra_process_comms(true);
    rclcpp::executors::SingleThreadedExecutor exec;

    auto map_builder = std::make_shared<cartographer::mapping::MapBuilder>(2000, 0.1);
    exec.add_node(map_builder);

    RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "\033[1;32m----> Map Builder Started.\033[0m");

    exec.spin();

    rclcpp::shutdown();
    return 0;
}